#ifndef ROBORTS_DECISION_ATTACK_ENEMY_BUFF_BEHAVIOR_H
#define ROBORTS_DECISION_ATTACK_ENEMY_BUFF_BEHAVIOR_H

#include "io/io.h"

#include "../blackboard/blackboard.h"
#include "../executor/chassis_executor.h"
#include "../behavior_tree/behavior_state.h"
#include "../proto/decision.pb.h"

#include "line_iterator.h"

namespace roborts_decision {
class AttackEnemyBuffBehavior{
  public:
  AttackEnemyBuffBehavior(ChassisExecutor* &chassis_executor,
		Blackboard* &blackboard,
	const std::string & proto_file_path) : chassis_executor_(chassis_executor),blackboard_(blackboard) {
	
	red_position[0].header.frame_id = "map";
	red_position[0].pose.position.x = 2.50;
    red_position[0].pose.position.y = 0.50;
    auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(0,0,0.56);//roll;pitch;yaw;
    red_position[0].pose.orientation = quaternion;
    
    red_position[1].header.frame_id = "map";
	red_position[1].pose.position.x = 1.10;
    red_position[1].pose.position.y = 3.90;
    quaternion = tf::createQuaternionMsgFromRollPitchYaw(0,0,-0.56);//roll;pitch;yaw;
    red_position[1].pose.orientation = quaternion;
    
    red_position[2] = red_position[1];
    red_position[3] = red_position[1];
    
    blue_position[0].header.frame_id = "map";
	blue_position[0].pose.position.x = 8.08 - red_position[3].pose.position.x;// = 7.06;
    blue_position[0].pose.position.y = 4.48 - red_position[3].pose.position.y;// = 0.78
    quaternion = tf::createQuaternionMsgFromRollPitchYaw(0,0,2.58);//roll;pitch;yaw;
    blue_position[0].pose.orientation = quaternion;
    
    blue_position[1] = blue_position[0];
    blue_position[2] = blue_position[0];
    
    blue_position[3].header.frame_id = "map";
	blue_position[3].pose.position.x = 8.08 - red_position[0].pose.position.x;// = 7.06;
    blue_position[3].pose.position.y = 4.48 - red_position[0].pose.position.y;// = 0.78
    quaternion = tf::createQuaternionMsgFromRollPitchYaw(0,0,-2.58);//roll;pitch;yaw;
    blue_position[3].pose.orientation = quaternion;
    
    a.twist.angular.z = 2;
    count = 0;
  }

  void Cancel() {
    chassis_executor_->Cancel();
  }

  BehaviorState Update() {
    return chassis_executor_->Update();
  }

  void Run() {
    int enemy_buff_position = blackboard_->GetEnemyBulletSupplyBuffInfo();
    int my_color = blackboard_->GetMyColor();
    geometry_msgs::PoseStamped attack_position;
    geometry_msgs::PoseStamped myposition = blackboard_->GetRobotMapPose();
    //enemy_buff_position = 3;
    //my_color = 2;
    if(my_color == 1) {
      attack_position = red_position[enemy_buff_position-3];
    }
    else {
      attack_position = blue_position[enemy_buff_position-1];
    }
    double distance = blackboard_->GetDistance(attack_position, myposition);
    auto executor_state = Update();
    if (in_circle==0 && executor_state != BehaviorState::RUNNING && executor_state != BehaviorState::FAILURE) {
      int time = blackboard_->GetGameRemainTime();
      time = time % 60;
      if(blackboard_->GetGameRemainTime() > 57)  {
        count = 0;
        chassis_executor_->Execute(attack_position, Behaviortype::ATTACK_ENEMY_BUFF);
      }
      if(distance > 0.5) {
        count = 0;
        chassis_executor_->Execute(attack_position, Behaviortype::ATTACK_ENEMY_BUFF);
      }
      else if (executor_state != BehaviorState::RUNNING) {
        a.twist.angular.z = 2 * cos(6.28* count / 200);
        chassis_executor_->Execute(a); 
        count++;
        count = count % 200;
      }
    }
    else if(in_circle||executor_state == BehaviorState::FAILURE) {
      if(in_circle==0) {
        begin_time = ros::Time::now();
      }
      in_circle=1;
      roborts_msgs::TwistAccel t;
      t.twist.angular.z = 1.57;
      chassis_executor_->Execute(t);
      if(ros::Time::now().sec-begin_time.sec>=4)in_circle=0;
    }
  }

~AttackEnemyBuffBehavior(){Cancel();}

private:
  ChassisExecutor* const chassis_executor_;
  Blackboard* const blackboard_;
  geometry_msgs::PoseStamped red_position[4];//0-p3;1-p4;2-p5;3-p6;
  geometry_msgs::PoseStamped blue_position[4];//0-p1;1-p2;2-p3;3-p4;
  roborts_msgs::TwistAccel a;
  int count;
  int in_circle;
  ros::Time begin_time;
};
}
#endif
